﻿#include "pclfun.h"
#include "eigenfuncset.h"

#define PI 3.14159265358979323846264338

PclFun::PclFun()
{}

PclFun::~PclFun()
{}

// 点云显示 https://blog.csdn.net/weixin_42585456/article/details/121176411
// https :www.zhihu.com/question/441275484/answer/2644053537
void PclFun::ShowCloud(const PointCloud::Ptr& pcloud, const std::string& sViewerNam)
{		
	pcl::visualization::PCLVisualizer visualizer(sViewerNam);
	visualizer.setBackgroundColor(0, 0, 0);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> sig(pcloud, 0, 234, 0);
	visualizer.addPointCloud<pcl::PointXYZ>(pcloud, sig, "PointCloud");
	//visualizer.setPointCloudRenderingProperties(
		//pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "PointCloud");
	//visualizer.addCoordinateSystem(5.0);

	while (!visualizer.wasStopped()) {
		visualizer.spinOnce();
	}
}

PointCloud::Ptr PclFun::ExtractIndicesCloud(const PointCloud::Ptr& pCloud,
	const pcl::PointIndices::Ptr& inliers)
{
	PointCloud::Ptr pCloudExtract(new PointCloud());
	pcl::ExtractIndices<pcl::PointXYZ> extract;//点云提取对象
		// 分离内层
	extract.setInputCloud(pCloud);//设置输入点云
	extract.setIndices(inliers);//设置分割后的内点点集
	extract.setNegative(false);//设置提取内点 而非外点
	extract.filter(*pCloudExtract);//提取

	return pCloudExtract;
}


/****************点云分析**************/
// 平面分割
PointCloud::Ptr PclFun::PlaneSegmentation(const PointCloud::Ptr& pCloud, float fMaxDist, pcl::ModelCoefficients::Ptr pcoefficients)
{ 
	pcl::ModelCoefficients coefficients; // 模型系数对象 此处即平面系数
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices); // 存内点索引集合对象
	//创建分割对象
	pcl::SACSegmentation<pcl::PointXYZ> seg;
	seg.setOptimizeCoefficients(true); //可选：设置模型系数优化
	seg.setModelType(pcl::SACMODEL_PLANE); // 必须:模型类型
	seg.setMethodType(pcl::SAC_RANSAC); // 随机参数估计方法
	//float fMaxDist = 0.001;// 0.006;
	seg.setDistanceThreshold(fMaxDist);///距离阈值
	seg.setInputCloud(pCloud);//输入点云
	seg.segment(*inliers, *pcoefficients);//分割
	return ExtractIndicesCloud(pCloud, inliers);
	
}

// 平面系数转欧拉角 std::vector<float>值分别代表（YA, XA，ZA, distance[原点到平面距离]）
std::vector<float> PclFun::PlaneCoefficients2Euler(const pcl::ModelCoefficients::Ptr& coefficients)
{
	std::vector<float> ret;
	ret.push_back(std::acos(coefficients->values[0]) / PI * 180);	// pfAlpha-90绕Y轴的角度
	ret.push_back(std::acos(coefficients->values[1]) / PI * 180);	// pfBeta - 90绕X轴的角度
	ret.push_back(std::acos(coefficients->values[2]) / PI * 180);
	ret.push_back(coefficients->values[3]);
	return ret;
}

// 剪切特定方向点云 // 根据向量方向进行裁剪
PointCloud::Ptr PclFun::CropCloudNormalRange(PointCloud::Ptr& pCloud, float fNormalAngleRange, char Axis)
{
	PointCloud::Ptr o3dPointCloud_result(new PointCloud);
	if (pCloud->size() == 0) {
		return nullptr;
	}
	pcl::PointXYZ xyz;
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
	tree->setInputCloud(pCloud);
	n.setInputCloud(pCloud);
	n.setSearchMethod(tree);
	n.setKSearch(10);
	n.compute(*normals);//swx:计算法向量。

	float fNormalAngleRange_rad = fNormalAngleRange * PI / 180;

	if (Axis == 'z' || Axis == 'Z') {
		for (int i = 0; i < pCloud->size(); i++) {
			float normal_x = normals->points[i].normal_x;
			float normal_y = normals->points[i].normal_y;
			float normal_z = normals->points[i].normal_z;
			float curvature = normals->points[i].curvature;//曲率
			float z_normal_angle = abs(std::atan((sqrt(normal_x * normal_x + normal_y * normal_y) / normal_z)));//swx:平面与向量的角度

			if (z_normal_angle < fNormalAngleRange_rad) {
				xyz.x = pCloud->points[i].x;
				xyz.y = pCloud->points[i].y;
				xyz.z = pCloud->points[i].z;
				o3dPointCloud_result->points.push_back(xyz);
			}
		}
	} else if (Axis == 'y' || Axis == 'Y') {
		for (int i = 0; i < pCloud->size(); i++) {
			float normal_x = normals->points[i].normal_x;
			float normal_y = normals->points[i].normal_y;
			float normal_z = normals->points[i].normal_z;
			float curvature = normals->points[i].curvature;//曲率
			float z_normal_angle = abs(std::atan((sqrt(normal_x * normal_x + normal_z * normal_z) / normal_y)));//swx:平面与向量的角度

			if (z_normal_angle < fNormalAngleRange_rad) {
				xyz.x = pCloud->points[i].x;
				xyz.y = pCloud->points[i].y;
				xyz.z = pCloud->points[i].z;
				o3dPointCloud_result->points.push_back(xyz);
			}
		}
	} else if (Axis == 'x' || Axis == 'X') {
		for (int i = 0; i < pCloud->size(); i++) {
			float normal_x = normals->points[i].normal_x;
			float normal_y = normals->points[i].normal_y;
			float normal_z = normals->points[i].normal_z;
			float curvature = normals->points[i].curvature;//曲率
			float z_normal_angle = abs(std::atan((sqrt(normal_y * normal_y + normal_z * normal_z) / normal_x)));//swx:平面与向量的角度

			if (z_normal_angle < fNormalAngleRange_rad) {
				xyz.x = pCloud->points[i].x;
				xyz.y = pCloud->points[i].y;
				xyz.z = pCloud->points[i].z;
				o3dPointCloud_result->points.push_back(xyz);
			}
		}
	}
	return o3dPointCloud_result;
}

PointCloud::Ptr PclFun::GetMaxCloud(std::vector<PointCloud::Ptr> vCloudPtr)
{
	if (vCloudPtr.size() == 0) {
		return nullptr;
	}
	//最大点云
	int iMaxCloudIndex = 0;
	int iMaxSize = 0;
	for (int i = 0; i < vCloudPtr.size(); i++) {
		int isize = vCloudPtr[i]->points.size();
		if (isize > iMaxSize) {
			iMaxSize = isize;
			iMaxCloudIndex = i;
		}
	}
	return vCloudPtr[iMaxCloudIndex];
}

//PCL点云转Opencv点云
cv::Mat PclFun::PCLCloud2cvCloudMask(PointCloud::Ptr& pCloud, const int& iImageRows, const int& iImageClos,
	const cv::Mat& matCameraMatrix, const cv::Mat& matDistCoeffs)
{
	//3D点云数据转化
	std::vector<cv::Point3f> CloudPoints;//空间点
	CloudPoints.reserve(pCloud->points.size());//预分配空间
	for (const auto& point : pCloud->points) {
		if (!std::isnan(point.z)) {
			CloudPoints.push_back(cv::Point3f(point.x, point.y, point.z));
		}
	}

	//把点投影到2D平面上
	//相机位置
	Eigen::Matrix4d CamPose = EigenFunc::TransEulerPostion2Matrix({ 0, 0, 0, 0, 0, 0 });
	cv::Mat CamPoseMat = EigenFunc::MatrixToMat4d(CamPose);
	cv::Mat Rvec, Tvec;
	EigenFunc::HomMat3dToRVT(CamPoseMat, &Rvec, &Tvec);

	std::vector<cv::Point2f> ImagePoints;//绘制箭头投影图像点
	cv::projectPoints(CloudPoints, Rvec, Tvec, matCameraMatrix, matDistCoeffs, ImagePoints);

	//点云赋值
	cv::Mat cvCloud_mask = cv::Mat::zeros(iImageRows, iImageClos, CV_8UC1);//有序点云
	for (int i = 0; i < ImagePoints.size(); i++) {
		const cv::Point2f& point = ImagePoints[i];
		int px = round(point.x);
		int py = round(point.y);
		if (px > 0 && px < iImageClos && py>0 && py < iImageRows) {
			cvCloud_mask.at<uchar>(py, px) = 255;
		}
	}

	return cvCloud_mask;
}

Point PclFun::CloudOBB(const PointCloud::Ptr& cloud, Eigen::Matrix3f& rotationalMatrix,
	Eigen::Vector3f& size)
{
	//计算惯性矩描述子
	pcl::MomentOfInertiaEstimation <pcl::PointXYZ> moment;//惯性与偏心率描述子类对象
	moment.setInputCloud(cloud);
	moment.compute();	// 开始计算
	Point min_obb, max_obb, pos_obb;	// 位置信息
	moment.getOBB(min_obb, max_obb, pos_obb, rotationalMatrix);

	size = Eigen::Vector3f(max_obb.x - min_obb.x, max_obb.y - min_obb.y, max_obb.z - min_obb.z);
	return pos_obb;
}
